#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

extern float hand_len;

void Matrix_Init();
bool leftArmInverse(Eigen::Vector3d position, Eigen::Vector3d orientation, Eigen::VectorXd &current_joints, bool hand_flag = true);
bool rightArmInverse(Eigen::Vector3d position, Eigen::Vector3d orientation, Eigen::VectorXd &current_joints, bool hand_flag = true);